// simplified slam for easy maze
//  by Kyohei

#include <assert.h>
#include <vector>

#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include "navigation/IR_conv.h"

#include <Eigen3/Dense>

//#include "Map.h"
#include "mzslam.h"

using namespace std;
using namespace Eigen3;

namespace mzslam
{
MzSLAM::MzSLAM()
{
  // parameter init
  x_ << 0, 0, 0; // initial pose
  px_ << 0, 0, 0; // initial pose
  z_.resize(0, 2);
  pz_.resize(0, 2);

  // Read local parameters
  //ros::NodeHandle local_nh("~");

  /* subscriber */
  ros::NodeHandle nh;
  pose_sub_ = nh.subscribe("/robot/pose", 100, &MzSLAM::update_pose, this);
  ir_sub_ = nh.subscribe("/IR/ir_converted", 1000, &MzSLAM::register_walls, this);

  /* publisher */
  //map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/mapping/map", 10);
  //map_meta_pub_ = nh.advertise<nav_msgs::MapMetaData>("/mapping/map_metadata", 10);
  scan_pub_ = nh.advertise<sensor_msgs::LaserScan>("/mapping/IRscan", 100);
}

MzSLAM::~MzSLAM()
{
}

void MzSLAM::update_pose(const geometry_msgs::Pose2D::ConstPtr& msg)
{
  px_ = x_;
  x_[0] = msg->x;
  x_[1] = msg->y;
  x_[2] = msg->theta;

  // braodcast tfMessage
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  transform.setRotation( tf::Quaternion(0, 0, msg->theta-1.57) );
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/odom", "/robot/base_link"));
}

void MzSLAM::register_walls(const navigation::IR_conv::ConstPtr &msg)
{
  MatrixXf ir(3, 3);

  //! convert IR readings
  _convert_IRs(msg, ir);

  //! estimate wall
  Matrix2f walls[3]; // [ x1 y1; x2 y2 ]
  pz_ = z_;
  z_.resize(0, 2); // [ a_front r_front; ... ; a_right r_right ]
  sensor_msgs::LaserScan scan;
  bool pub = false;
  FOR(i, 0, 3) {
    if( estimate_wall(ir, i, walls[i], z_) ) {
      _convert_wall_to_scan(walls[i], scan);
      pub = true;
    }
  }
  // DBG
  ROS_INFO_STREAM("FrontWall: " << endl << walls[FRONT]);
  ROS_INFO_STREAM("LeftWall: " << endl << walls[LEFT]);
  ROS_INFO_STREAM("RightWall: " << endl <<  walls[RIGHT]);

  // @todo theta can be corrected here??

  //TODO implement SLAM here
  // x_, px_: robot's current pose (x, y, theta)
  // z_, pz_: measurement vector(polar coordinate)

  // measurement prediction (from previous measurment)

  // matching

  // estimation and publish predicted pose


  // publish scanner
  if(pub) scan_pub_.publish(scan);
}

} // end of namespace

int main(int argc, char **argv)
{
  ros::init(argc, argv, "mzslam");
  mzslam::MzSLAM mzslam_node;

  ros::spin();
  return 0;
}
